//////////////////////////////////////////////////////////////////////
//
//  CryEngine Source code
//	
//	File:Skeleton.cpp
//  Implementation of Skeleton class (Forward Kinematics)
//
//	History:
//	January 12, 2005: Created by Ivo Herzeg <ivo@crytek.de>
//
//////////////////////////////////////////////////////////////////////
#include "StdAfx.h"
#include <I3DEngine.h>
#include <IRenderAuxGeom.h>
#include "CharacterInstance.h"
#include "Model.h"
#include "ModelSkeleton.h"
#include "CharacterManager.h"
#include <float.h>
#include "FacialAnimation/FacialInstance.h"
#include "PoseModifier/PoseModifierHelper.h"
#include "PoseModifier/GroundAlignment.h"
#include "PoseModifier/Recoil.h"

void CSkeletonPose::UpdateAttachments(Skeleton::CPoseData& poseData, const QuatT& locationPhysics, const QuatTS& locationAnimation, float fZoomAdjustedDistanceFromCamera, bool onRender)
{
#ifndef _RELEASE	
	poseData.ValidateRelPose(m_parrModelJoints);
	poseData.ValidateAbsPose(m_parrModelJoints);
#endif

	m_pInstance->m_AttachmentManager.SetPoseData(&poseData);

	if (m_pInstance->IsCharacterVisible() || m_bFullSkeletonUpdate)
	{
		m_pInstance->UpdateAttachedObjects(locationPhysics, 0, fZoomAdjustedDistanceFromCamera, onRender);
		UpdateBBox();
	}
	else
	{
		m_pInstance->UpdateAttachedObjectsFast(locationPhysics, fZoomAdjustedDistanceFromCamera, onRender);
	}

	m_pInstance->m_AttachmentManager.SetPoseData(NULL);
}

void CSkeletonPose::SkeletonPostProcess(Skeleton::CPoseData& poseData, const QuatT& rPhysLocationNext, const QuatTS &rAnimLocationNext, IAttachment* pIAttachment, float fZoomAdjustedDistanceFromCamera, uint32 OnRender)
{
	DEFINE_PROFILER_FUNCTION();

	CCharInstance* const __restrict pInstance = m_pInstance;
	CSkeletonAnim* const __restrict pSkeletonAnim = m_pSkeletonAnim;
	int nCurrentFrameID = g_pCharacterManager->m_nUpdateCounter; 
	if (pSkeletonAnim->GetCharEditMode() == 0)
	{
		if (pInstance->m_LastUpdateFrameID_Post == nCurrentFrameID)
			return;
		}

	//

	QuatT* const __restrict pRelativePose = &poseData.m_pJointsRelative[0];
	QuatT* const __restrict pAbsolutePose = &poseData.m_pJointsAbsolute[0];

#ifndef _RELEASE	
	poseData.ValidateRelPose(m_parrModelJoints);
#endif

	// -------------------------------------------------------------------------

	m_pInstance->m_PhysEntityLocation = rPhysLocationNext;
	if (m_bPhysicsRelinquished)
		m_pInstance->m_PhysEntityLocation.q.SetIdentity();
	m_pInstance->m_fOriginalDeltaTime = g_pITimer->GetFrameTime();
	m_pInstance->m_fDeltaTime = m_pInstance->m_fOriginalDeltaTime * m_pInstance->m_fAnimSpeedScale;

	// TODO: Needed for decoupled method, try to remove this!
	uint32 objectType =	pInstance->m_pModel->m_ObjectType;
	if (m_bFullSkeletonUpdate)
	{
		if ((pSkeletonAnim->m_IsAnimPlaying&1) && pSkeletonAnim->m_AnimationDrivenMotion && objectType==CHR)
		{
			pRelativePose[0] = rPhysLocationNext.GetInverted()*QuatT(rAnimLocationNext);
		}
	}

#ifndef _RELEASE	
	poseData.ValidateRelPose(m_parrModelJoints);
#endif

	if (pSkeletonAnim->m_IsAnimPlaying || m_bFullSkeletonUpdate)
	{
		if (m_pPostProcessCallback)		
			(*m_pPostProcessCallback)(m_pInstance, m_pPostProcessCallbackData);

		bool bComputeAbsolutePose = false;
		uint32 numEntries = m_arrPostProcess.size();
		if (numEntries)
		{
			for (uint32 i=0; i<numEntries; i++)
			{
				int32 idx = m_arrPostProcess[i].m_JointIdx;
				QuatT qt  = m_arrPostProcess[i].m_RelativePose;
				if (!QuatT::IsEquivalent(pRelativePose[idx], qt, 0.0001f))
				{
					pRelativePose[idx] = qt;
					pSkeletonAnim->m_IsAnimPlaying |= 0x80000000;
					bComputeAbsolutePose = true;
				}
			}
		}
		m_arrPostProcess.resize(0);

#ifndef _RELEASE	
		poseData.ValidateRelPose(m_parrModelJoints);
#endif

		// if there were are DispacementInfos resulting from the update, apply them now to the bones, 
		// all the data that has to be accessed is assembled and a static function is called
		if (pSkeletonAnim->m_facialDisplaceInfo.HasUsed())
		{
			CFacialInstance* pFacialInstance = pInstance->m_pFacialInstance;
			CSkeletonPose* pSkeletonPose;
			if (pFacialInstance)
				pSkeletonPose = &pFacialInstance->GetMasterCharacter()->m_SkeletonPose;
			else
				pSkeletonPose = &pInstance->m_SkeletonPose;

			const QuatT* const pJointsRelativeDefault = &pSkeletonPose->GetPoseDataDefault().m_pJointsRelative[0];
			CFacialModel::ApplyDisplaceInfoToJoints(pSkeletonAnim->m_facialDisplaceInfo, m_pModelSkeleton, &poseData, pJointsRelativeDefault);
		}

#ifndef _RELEASE	
		poseData.ValidateRelPose(m_parrModelJoints);
#endif
		poseData.ComputeAbsolutePose(m_parrModelJoints, m_pInstance->m_pModel->m_ObjectType == CHR);
#ifndef _RELEASE	
		poseData.ValidateAbsPose(m_parrModelJoints);
#endif
	}

#ifndef _RELEASE	
	poseData.ValidateRelPose(m_parrModelJoints);
#endif

	SynchronizeWithPhysics(poseData, rPhysLocationNext, rAnimLocationNext, pIAttachment ? pIAttachment->GetAttRelativeDefault().GetInverted() : QuatT(IDENTITY));
	m_pSkeletonAnim->PoseModifiersExecutePost(poseData, rPhysLocationNext, rAnimLocationNext);

	if (m_pInstance->IsCharacterVisible() || m_bFullSkeletonUpdate)
	{
		m_pInstance->BeginSkinningTransformationsComputation();
	}

	m_pSkeletonAnim->PoseModifiersSynchronize();

	SynchronizeWithPhysicsPost(rPhysLocationNext, rAnimLocationNext);

#ifndef _RELEASE	
	poseData.ValidateRelPose(m_parrModelJoints);
	poseData.ValidateAbsPose(m_parrModelJoints);
#endif

	// -------------------------------------------------------------------------


#if !defined(__SPU__)
	DrawPositionPost(rPhysLocationNext, rAnimLocationNext);

	if (Console::GetInst().ca_DebugAnimUpdates)
	{
		//just for debugging
		stack_string strModelPath = m_pInstance->m_pModel->GetModelFilePath();
		g_pCharacterManager->m_arrSkeletonUpdates.push_back(strModelPath);
		g_pCharacterManager->m_arrAnimPlaying.push_back(pSkeletonAnim->m_IsAnimPlaying);
		g_pCharacterManager->m_arrForceSkeletonUpdates.push_back(m_nForceSkeletonUpdate);
		g_pCharacterManager->m_arrVisible.push_back(m_bInstanceVisible);
	}
#endif

#ifndef _RELEASE	
	poseData.ValidateRelPose(m_parrModelJoints);
	poseData.ValidateAbsPose(m_parrModelJoints);
#endif
	UpdateAttachments(poseData, rPhysLocationNext, rAnimLocationNext, fZoomAdjustedDistanceFromCamera, !!OnRender);

#if !defined(__SPU__) 
	//	if (Console::GetInst().ca_DrawBodyMoveDir && pSkeletonAnim->m_AnimationDrivenMotion)
	if (Console::GetInst().ca_DrawLocator)
		DrawLocator(rAnimLocationNext, Console::GetInst().ca_DrawLocator);
#endif

	pInstance->m_LastUpdateFrameID_PostType = OnRender;
	if (OnRender!=1)
		pInstance->m_LastUpdateFrameID_Post=nCurrentFrameID;

	if (pSkeletonAnim->m_AnimationDrivenMotion && objectType==CHR)
		pRelativePose[0].SetIdentity();

	if ( (m_nForceSkeletonUpdate&0x8000)==0)
	{
		m_nForceSkeletonUpdate--;
		if (m_nForceSkeletonUpdate<0)	m_nForceSkeletonUpdate=0;
	}

// 	if (m_pPostProcessCallback)		
// 		(*m_pPostProcessCallback)(m_pInstance, m_pPostProcessCallbackData);

	m_pPoseDataWriteable = NULL;
}

#if defined(_RELEASE)
#define BBOX_ERROR_CHECKING 0
#else
#define BBOX_ERROR_CHECKING 1
#endif

void CSkeletonPose::UpdateBBox(uint32 update)
{
	DEFINE_PROFILER_FUNCTION();

	const QuatT* const __restrict pAbsolutePose = GetPoseData().GetJointsAbsolute();

#ifndef _RELEASE	
	GetPoseData().ValidateRelPose(m_parrModelJoints);
	GetPoseData().ValidateAbsPose(m_parrModelJoints);
#endif

	// cache m_AABB to stack on SPU




	AABB rAABB;


	uint32 nErrorCode=0;
	const f32 fMaxBBox=13000.0f;
	rAABB.min.Set(+99999.0f,+99999.0f,+99999.0f);
	rAABB.max.Set(-99999.0f,-99999.0f,-99999.0f);

	//g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColor, false,"Update BBox:  NumBones:%d   Model: %s", numJoints, m_pInstance->GetModelFilePath() );	
	//g_YLine+=16.0f;

	const uint32 numCGAJoints = m_arrCGAJoints.size();
	const uint32 numJoints = GetJointCount();
	if (numCGAJoints)
	{
		//if we have a CGA, then us this code
		for (uint32 i=0; i<numCGAJoints; ++i) 
		{
			if (m_arrCGAJoints[i].m_CGAObjectInstance)
			{
				QuatT bone = pAbsolutePose[i];
				AABB aabb = AABB::CreateTransformedAABB(Matrix34(bone),m_arrCGAJoints[i].m_CGAObjectInstance->GetAABB());
				rAABB.Add(aabb);
			}
		}
	}
	else
	{
		//if we have a CHR, then us this code
		for (uint32 i=1; i<numJoints; i++)
		{
			rAABB.Add( pAbsolutePose[i].t );
#ifdef _DEBUG
			assert(fabsf(pAbsolutePose[i].t.x) < 3000.0f);
			assert(fabsf(pAbsolutePose[i].t.y) < 3000.0f);
			assert(fabsf(pAbsolutePose[i].t.z) < 3000.0f);
#endif			
		}

	}

	if (numCGAJoints==0 && numJoints<=1)
	{
		rAABB.min=Vec3(-2,-2,-2);
		rAABB.max=Vec3( 2, 2, 2);
	}

	//----------------------------------------------------------------------------------------------------------
#if BBOX_ERROR_CHECKING
	uint32 minValid = rAABB.min.IsValid();
	if (minValid==0) nErrorCode|=0x4000;
	uint32 maxValid = rAABB.max.IsValid();
	if (maxValid==0) nErrorCode|=0x4000;
#endif
	//----------------------------------------------------------------------------------------------------------

	CAttachmentManager* pAttachmentManager = static_cast<CAttachmentManager*>(m_pInstance->GetIAttachmentManager());
	if (pAttachmentManager)
	{
		const uint32 numAttachments = pAttachmentManager->GetAttachmentCount();
		if(numAttachments > 0)
		{
			CAttachment ** pAttachmentArray = pAttachmentManager->GetInterfaceArray();
			for (uint32 i=0; i<numAttachments; i++)
			{
				CAttachment* pAttachment = pAttachmentArray[i];
				if (pAttachment)
				{
					const char *pAttName = pAttachment->GetName();
					bool bAttachmentForBoneLink = pAttName!=0 && 
						strncmp(pAttName, BONE_ATTACH_ENTITY_LINK_PREFIX, strlen(BONE_ATTACH_ENTITY_LINK_PREFIX)) == 0;
					if ( (pAttachment->m_Type==CA_BONE || pAttachment->m_Type==CA_FACE) 
						&& (pAttachment->CAttachment::IsAttachmentHidden()==0) 
						&& !bAttachmentForBoneLink)
					{

						IAttachmentObject* pIAttachmentObject = pAttachment->CAttachment::GetIAttachmentObject();
						if (pIAttachmentObject)
						{
							IAttachmentObject::EType eAttachmentType = pIAttachmentObject->GetAttachmentType();
							uint32 e = ( eAttachmentType == IAttachmentObject::eAttachment_Entity);
							uint32 c = ( eAttachmentType == IAttachmentObject::eAttachment_Character);
							uint32 s = ( eAttachmentType == IAttachmentObject::eAttachment_StatObj);

							if( e | c | s)
							{	
								AABB aabb = pIAttachmentObject->GetAABB();

#if BBOX_ERROR_CHECKING
								minValid =	aabb.min.IsValid();
								maxValid =	aabb.max.IsValid();
								if (minValid==0 || maxValid==0)
								{
									if( e ) AnimWarning("CryAnimation: Invalid BBox of eAttachment_Entity" );
									if( c ) AnimWarning("CryAnimation: Invalid BBox of eAttachment_Character" );
									if( s ) AnimWarning("CryAnimation: Invalid BBox of eAttachment_StatObj" );
									assert(0);
								}

								nErrorCode=0;
								if (aabb.max.x < aabb.min.x) nErrorCode|=0x0001;
								if (aabb.max.y < aabb.min.y) nErrorCode|=0x0001;
								if (aabb.max.z < aabb.min.z) nErrorCode|=0x0001;
								if (aabb.min.x < -fMaxBBox) nErrorCode|=0x0002;
								if (aabb.min.y < -fMaxBBox) nErrorCode|=0x0004;
								if (aabb.min.z < -fMaxBBox) nErrorCode|=0x0008;
								if (aabb.max.x > +fMaxBBox) nErrorCode|=0x0010;
								if (aabb.max.y > +fMaxBBox) nErrorCode|=0x0020;
								if (aabb.max.z > +fMaxBBox) nErrorCode|=0x0040;

								assert(nErrorCode==0);
								if (nErrorCode)
								{
									if( e )
									{
										IStatObj* pIStatObj =  pIAttachmentObject->GetIStatObj();
										if (pIStatObj)
										{
											const char* strFolderName = pIStatObj->GetFilePath();
											AnimWarning("CryAnimation: Invalid BBox of eAttachment_Entity (%.3f,%.3f,%.3f)-(%.3f,%.3f,%.3f). ModenPath: '%s' ObjPathName '%s'", 	aabb.min.x, aabb.min.y, aabb.min.z, aabb.max.x, aabb.max.y, aabb.max.z, 	m_pInstance->m_pModel->GetModelFilePath(), strFolderName);
										}
										else
										{
											AnimWarning("CryAnimation: Invalid BBox of eAttachment_Entity (%.3f,%.3f,%.3f)-(%.3f,%.3f,%.3f). ModenPath: '%s' ", 	aabb.min.x, aabb.min.y, aabb.min.z, aabb.max.x, aabb.max.y, aabb.max.z, 	m_pInstance->m_pModel->GetModelFilePath());
										}
										assert(0);
									}

									if( c )	AnimWarning("CryAnimation: Invalid BBox of eAttachment_Character (%.3f,%.3f,%.3f)-(%.3f,%.3f,%.3f). ModenPath: '%s'  ErrorCode: %08x", 	aabb.min.x, aabb.min.y, aabb.min.z, aabb.max.x, aabb.max.y, aabb.max.z, 	m_pInstance->m_pModel->GetModelFilePath(), nErrorCode);
									if( s ) AnimWarning("CryAnimation: Invalid BBox of eAttachment_StatObj (%.3f,%.3f,%.3f)-(%.3f,%.3f,%.3f). ModenPath: '%s'  ErrorCode: %08x", 	aabb.min.x, aabb.min.y, aabb.min.z, aabb.max.x, aabb.max.y, aabb.max.z, 	m_pInstance->m_pModel->GetModelFilePath(), nErrorCode);
								}
#endif

								assert(aabb.min.IsValid());
								assert(aabb.max.IsValid());
							const QuatT& attrel=pAttachment->GetAttModelRelative();
							AABB taabb; taabb.SetTransformedAABB(attrel,aabb);
#ifdef _DEBUG
							assert(fabsf(taabb.min.x) < 3000.0f);
							assert(fabsf(taabb.min.y) < 3000.0f);
							assert(fabsf(taabb.min.z) < 3000.0f);
							assert(fabsf(taabb.max.x) < 3000.0f);
							assert(fabsf(taabb.max.y) < 3000.0f);
							assert(fabsf(taabb.max.z) < 3000.0f);
#endif			

								assert(taabb.min.IsValid());
								rAABB.Add(taabb.min);

								assert(taabb.max.IsValid());
								rAABB.Add(taabb.max);
							}
						}
					}
				}
			}
		}
	}

#if BBOX_ERROR_CHECKING
	if (rAABB.max.x < rAABB.min.x) nErrorCode=0x0001;
	if (rAABB.max.y < rAABB.min.y) nErrorCode=0x0001;
	if (rAABB.max.z < rAABB.min.z) nErrorCode=0x0001;

	minValid = rAABB.min.IsValid();
	if (minValid==0) nErrorCode|=0x8000;
	maxValid = rAABB.max.IsValid();
	if (maxValid==0) nErrorCode|=0x8000;


	if (rAABB.min.x < -fMaxBBox) nErrorCode|=0x0002;
	if (rAABB.min.y < -fMaxBBox) nErrorCode|=0x0004;
	if (rAABB.min.z < -fMaxBBox) nErrorCode|=0x0008;
	if (rAABB.max.x > +fMaxBBox) nErrorCode|=0x0010;
	if (rAABB.max.y > +fMaxBBox) nErrorCode|=0x0020;
	if (rAABB.max.z > +fMaxBBox) nErrorCode|=0x0040;
	assert(nErrorCode==0);
#endif

	const float fSelectX = (rAABB.max.x - rAABB.min.x) - 0.4f;
	const float fSelectY = (rAABB.max.y - rAABB.min.y) - 0.4f;
	const float fSelectZ = (rAABB.max.z - rAABB.min.z) - 0.4f;

	rAABB.max.x = (float)__fsel(fSelectX, rAABB.max.x, rAABB.max.x + 0.2f);
	rAABB.min.x = (float)__fsel(fSelectX, rAABB.min.x, rAABB.min.x - 0.2f);
	rAABB.max.y = (float)__fsel(fSelectY, rAABB.max.y, rAABB.max.y + 0.2f);
	rAABB.min.y = (float)__fsel(fSelectY, rAABB.min.y, rAABB.min.y - 0.2f);
	rAABB.max.z = (float)__fsel(fSelectZ, rAABB.max.z, rAABB.max.z + 0.2f);
	rAABB.min.z = (float)__fsel(fSelectZ, rAABB.min.z, rAABB.min.z - 0.2f);

#if BBOX_ERROR_CHECKING
	if (nErrorCode)
	{
		AnimWarning("CryAnimation: Invalid BBox (%.3f,%.3f,%.3f)-(%.3f,%.3f,%.3f). ModenPath: '%s'  ErrorCode: %08x", 	rAABB.min.x, rAABB.min.y, rAABB.min.z, rAABB.max.x, rAABB.max.y, rAABB.max.z, 	m_pInstance->m_pModel->GetModelFilePath(), nErrorCode);
		assert(!"Invalid BBox");
		rAABB.min=Vec3(-2,-2,-2);
		rAABB.max=Vec3( 2, 2, 2);
	}
#endif

#ifndef __SPU__
	m_AABB = rAABB;
#endif
}

void CSkeletonPose::AdjustFeetHelper( QuatT &rAbsoluteCalf, QuatT &rRelativeCalf, QuatT &rAbsoluteTight, QuatT &rAbsoluteFoot,QuatT &rRelativeFoot, const Vec3 &rCross, const Vec3 &rKneeDir, uint32 iteration, float knee_factor )
{
	for (uint32 i=0; i<iteration; i++)
	{
		Vec3 ulleg = rAbsoluteCalf.t - rAbsoluteTight.t;
		Vec3 llleg = rAbsoluteFoot.t - rAbsoluteCalf.t;

		Vec3 lcross = ulleg%llleg;

		if ( (lcross|lcross)>0.0001f && (rCross|lcross)>0)
			break;

		rAbsoluteCalf.t += rKneeDir*knee_factor;
	}

	rRelativeCalf = rAbsoluteTight.GetInverted() 	* rAbsoluteCalf;
	rRelativeFoot = rAbsoluteCalf.GetInverted() 	* rAbsoluteFoot;
}
